%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     Implementation of Rigid Body DCA
%     Author: Kishor Bhalerao
%     Date: 29th Jan 2007
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
%%% Two body spherical joint pendulum%

function dp

% Input, Body 1-2-3 Euler Angles
% q1 = [1 1 1];
% q2 = [1 1 1];
% u1 = [1 1 1];
% u2 = [1 1 1];
q1 = [1 2 3];
q2 = [1 2 3];
u1 = [1 2 3];
u2 = [1 2 3];

x0=[q1 q2 u1 u2]'; % Initial state

% -------------------------------------------------------------
% Fixed parameters
ma = 1;
La = 1;
Ia = [2.657092439688067,0.5990735762505496,-0.4537527488828099;
      0.5990735762505497,1.658531228755636,0.3335032909336348;
     -0.4537527488828099,0.3335032909336348,1.684376331556298];


mb = 1;
Lb = 1;

Ib = [2.657092439688067,0.5990735762505496,-0.4537527488828099;
      0.5990735762505497,1.658531228755636,0.3335032909336348;
     -0.4537527488828099,0.3335032909336348,1.684376331556298];
  
      
param = {ma;La;Ia;mb;Lb;Ib};
% -------------------------------------------------------------
options = odeset('RelTol',1e-7,'AbsTol',1e-8);
[t,x] = ode45(@base,[0 5],x0,options,param);
p = [t,x];
% save data p
% keyboard
for i = 1:length(t)
    E(i) = dpke(t(i),x(i,:)',param);
end

keyboard

function x_der = base(t,x,param)

q(1:3,1) = x(1:3);
u(:,1) = x(7:9);
m(1) = param{1};
La = param{2};
Iner(:,:,1) = param{3};

q(1:3,2) = x(4:6);
u(:,2) = x(10:12);
m(2) = param{4};
Lb = param{5};
Iner(:,:,2) = param{6};

% -------------------------------------------------------------
for i = 1:2
    c1(i) = cos(q(1,i));
    c2(i) = cos(q(2,i));
    c3(i) = cos(q(3,i));

    s1(i) = sin(q(1,i));
    s2(i) = sin(q(2,i));
    s3(i) = sin(q(3,i));
end

% -------------------------------------------------------------
% Transformation matrices
for i = 1:2
    pr_P_K(1:3,1:3,i) = [c2(i)*c3(i)                   -c2(i)*s3(i)                    s2(i);
                        s1(i)*s2(i)*c3(i)+s3(i)*c1(i) -s1(i)*s2(i)*s3(i)+c1(i)*c3(i) -s1(i)*c2(i);
                        -c1(i)*s2(i)*c3(i)+s3(i)*s1(i) c1(i)*s2(i)*s3(i)+c3(i)*s1(i)  c1(i)*c2(i);];
end

N_C_K(1:3,1:3,1) = pr_P_K(1:3,1:3,1);
N_C_K(1:3,1:3,2) = N_C_K(1:3,1:3,1)*pr_P_K(1:3,1:3,2);

% -------------------------------------------------------------
% Handles in Body Basis

cm2H1(:,1) = [-La/2 0 0]';
cm2H2(:,1) = [La/2 0 0]';

cm2H1(:,2) = [-Lb/2 0 0]';
cm2H2(:,2) = [Lb/2 0 0]';

% -------------------------------------------------------------
% Convert to Newtonian basis
for i = 1:2
    cmtoH1(:,i) = N_C_K(:,:,i)*cm2H1(:,i);
    cmtoH2(:,i) = N_C_K(:,:,i)*cm2H2(:,i);
    I(:,:,i) = N_C_K(:,:,i)*Iner(:,:,i)*(N_C_K(:,:,i)'); % This step is useless for the values of inertia selected  
end

% -------------------------------------------------------------
% Angular velocities in body basis
% Page 427 Spacecraft Dynamics, Kane and Levinson

% Kinematical differential equations
for i = 1:2    
    W_Kde_qdot(1:3,1:3,i) = [ c2(i)*c3(i) s3(i) 0;
                             -c2(i)*s3(i) c3(i) 0;
                              s2(i)       0     1;];
                          
%     cond(W_Kde_qdot(1:3,1:3,i))                          
%     pause;
                          
    Kde_dot(1:3,1:3,i) = [-s2(i)*c3(i)*u(2,i)-c2(i)*s3(i)*u(3,i) c3(i)*u(3,i) 0;
                          s2(i)*s3(i)*u(2,i)-c2(i)*c3(i)*u(3,i) -s3(i)*u(3,i) 0;
                          c2(i)*u(2,i)                            0           0;];
    
    Wb(1:3,i) = u(:,i);
end

% Convert Angular velocities to Newtonian Basis and frame.
WN(1:3,1) = Wb(:,1);
WN(1:3,2) = WN(:,1) + N_C_K(:,:,1)*Wb(:,2);

% -------------------------------------------------------------
% Shift Matrices
for i = 1:2
    cmtoH1cross(:,:,i) = [0           -cmtoH1(3,i) cmtoH1(2,i);
                          cmtoH1(3,i)  0           -cmtoH1(1,i);
                         -cmtoH1(2,i)  cmtoH1(1,i) 0          ];
                      
    cmtoH2cross(:,:,i) = [0           -cmtoH2(3,i) cmtoH2(2,i);
                          cmtoH2(3,i)  0           -cmtoH2(1,i);
                         -cmtoH2(2,i)  cmtoH2(1,i) 0          ];

    SfH1(:,:,i) = [eye(3) cmtoH1cross(:,:,i);zeros(3) eye(3)];
    SfH2(:,:,i) = [eye(3) cmtoH2cross(:,:,i);zeros(3) eye(3)];
end

% -------------------------------------------------------------
% Mass properties
M(1:6,1:6,1) = [I(:,:,1), zeros(3,3);zeros(3,3),m(1)*eye(3)]; 
M(1:6,1:6,2) = [I(:,:,2), zeros(3,3);zeros(3,3),m(2)*eye(3)];

for i = 1:2
    WN_cross(1:3,1:3,i) = [0        -WN(3,i) WN(2,i);
                           WN(3,i)   0      -WN(1,i);
                          -WN(2,i)   WN(1,i) 0;];
end

% -------------------------------------------------------------
% To form equations of motion about center of mass
Gravity = [0 0 0 0 -9.81 0]'; % This is the only external force on the body
% -------------------------------------------------------------
% Form equations of motion about the handles
 
for i = 1:2
    iM = inv(M(:,:,i)); 
    wxIw = [cross(WN(:,i),I(:,:,i)*WN(:,i));zeros(3,1)];
    psi3(1:6,i) = iM*(Gravity*m(i) - wxIw) ;
    
    phi1H1(1:6,1:6,i) = SfH1(:,:,i)'*iM*SfH1(:,:,i);
    phi2H1(1:6,1:6,i) = SfH1(:,:,i)'*iM*SfH2(:,:,i);
    phi3H1(1:6,i) =     SfH1(:,:,i)'*iM*(Gravity*m(i) - wxIw)  + [zeros(3,1);... 
          cross(WN(:,i),cross(WN(:,i),cmtoH1(:,i)))];
    
      
    phi1H2(1:6,1:6,i) = SfH2(:,:,i)'*iM*SfH1(:,:,i);
    phi2H2(1:6,1:6,i) = SfH2(:,:,i)'*iM*SfH2(:,:,i);
    phi3H2(1:6,i) =     SfH2(:,:,i)'*iM*(Gravity*m(i) - wxIw)  + [zeros(3,1);... 
          cross(WN(:,i),cross(WN(:,i),cmtoH2(:,i)))];


end        
% -------------------------------------------------------------
% For spherical joint

H = [eye(3); zeros(3)];
D = [zeros(3); eye(3)];

%  -------------------------------------------------------------
% Combine the two bodies
E12 = D'*(phi1H1(:,:,2)+phi2H2(:,:,1))*D;
iE12 = inv(E12);

% Some intermediate terms
X = D*iE12*D';
A = X*phi1H2(:,:,1);
B = -X*phi2H1(:,:,2);
C = X*(phi3H2(:,1)-phi3H1(:,2));

Psi11 = phi1H1(:,:,1) - phi2H1(:,:,1)*A;
Psi12 = -phi2H1(:,:,1)*B;
Psi13 = phi3H1(:,1) - phi2H1(:,:,1)*C;


Psi21 = phi1H2(:,:,2)*A;
Psi22 = phi1H2(:,:,2)*B + phi2H2(:,:,2);
Psi23 = phi1H2(:,:,2)*C + phi3H2(:,2);

% -------------------------------------------------------------
% Solve the equations of motion for the double pendulum fixed at one end
'Integration wont work'
keyboard
fPH2 = -(D'*Psi22*D \ D'*Psi23);
aPH2 = H'*(Psi22*D*fPH2 + Psi23);

aQH1 = H'*(Psi12*D*fPH2 + Psi13);
% aQH1 = AQH1(1:3,1);

% -------------------------------------------------------------
% To compute udots

u1dot = W_Kde_qdot(:,:,1)\(N_C_K(:,:,1)'*aPH2 - Kde_dot(:,:,1)*u(:,1));
 
% To convert the relative angular acceleration on body Q in frame P and basis P
alpha_Q_temp = aQH1-aPH2-cross(WN(:,1),WN(:,2));
u2dot = W_Kde_qdot(:,:,2)\(N_C_K(:,:,2)'*alpha_Q_temp-Kde_dot(:,:,2)*u(:,2));
x_der = [u(:,1);u(:,2);u1dot;u2dot];
